﻿#include "FITKAssInstance.h"

// Global data
#include "FITK_Kernel/FITKCore/FITKDataRepo.h"

// Data
#include "FITK_Kernel/FITKCore/FITKVec3DAlg.h"
#include "FITKAbsAlgorithmTools.h"
#include "FITKAbsInterfaceFactory.h"

#include <iostream>

namespace Interface
{
    //FITKTransformation::FITKTransformation()
    //{
    //    _Matrix = Eigen::Matrix3d::Identity();
    //    _Vector = Eigen::Vector3d::Zero();
    //}
    //FITKTransformation::FITKTransformation(const Eigen::Matrix3d & matrix, const Eigen::Vector3d & vector)
    //    : _Matrix(matrix), _Vector(vector)
    //{
    //}

    //FITKTransformation::FITKTransformation(const Eigen::Vector3d & translation)
    //    : _Matrix(Eigen::Matrix3d::Identity()), _Vector(translation)
    //{
    //}

    //FITKTransformation::FITKTransformation(double* axisOrigin, double* axisDir, const double angle)
    //{
    //    Eigen::Vector3d vector;
    //    vector << -axisOrigin[0], -axisOrigin[1], -axisOrigin[2];
    //    // 执行旋转
    //    const double radian = angle / 180. * FITK_PI;
    //    const double cosVal = std::cos(radian);
    //    const double sinVal = std::sin(radian);
    //    const double dirX = axisDir[0];
    //    const double dirY = axisDir[1];
    //    const double dirZ = axisDir[2];
    //    _Matrix << cosVal + dirX * dirX * (1. - cosVal), dirX * dirY * (1. - cosVal) - dirZ * sinVal, dirX * dirZ * (1. - cosVal) + dirY * sinVal,
    //        dirY * dirX * (1. - cosVal) + dirZ * sinVal, cosVal + dirY * dirY * (1. - cosVal), dirY * dirZ * (1. - cosVal) - dirX * sinVal,
    //        dirZ * dirX * (1. - cosVal) - dirY * sinVal, dirZ * dirY * (1. - cosVal) + dirX * sinVal, cosVal + dirZ * dirZ * (1 - cosVal);

    //    vector = _Matrix * vector;
    //    // 平移到原先的位置
    //    Eigen::Vector3d newVec;
    //    newVec << axisOrigin[0], axisOrigin[1], axisOrigin[2];
    //    _Vector = vector + newVec;
    //}

    //Eigen::Matrix3d FITKTransformation::matrix() const
    //{
    //    return _Matrix;
    //}

    //void FITKTransformation::setMatrix(const Eigen::Matrix3d & matrix)
    //{
    //    _Matrix = matrix;
    //}

    //Eigen::Vector3d FITKTransformation::vector() const
    //{
    //    return _Vector;
    //}

    //void FITKTransformation::setVector(const Eigen::Vector3d & vector)
    //{
    //    _Vector = vector;
    //}

    //bool FITKTransformation::isRotation(double* axisOrigin, double* axisDir, double& angle) const
    //{
    //    Eigen::Matrix3d eMat;
    //    auto           m = eMat.Identity() - _Matrix;
    //    Eigen::Matrix3d inverseMatrix = m.completeOrthogonalDecomposition().pseudoInverse();
    //    Eigen::Vector3d origin = inverseMatrix * _Vector;

    //    axisOrigin[0] = origin[0];
    //    axisOrigin[1] = origin[1];
    //    axisOrigin[2] = origin[2];

    //    // 计算方向余弦矩阵的迹
    //    double trace = _Matrix.trace();
    //    // 计算旋转角度
    //    angle = std::acos((trace - 1) / 2) / FITK_PI * 180.;

    //    // 计算旋转轴
    //    Eigen::Vector3d dir;
    //    dir << _Matrix(2, 1) - _Matrix(1, 2),
    //        _Matrix(0, 2) - _Matrix(2, 0),
    //        _Matrix(1, 0) - _Matrix(0, 1);

    //    // 归一化旋转轴
    //    dir.normalize();

    //    axisDir[0] = dir[0];
    //    axisDir[1] = dir[1];
    //    axisDir[2] = dir[2];

    //    return true;
    //}

    //FITKTransformation operator*(const FITKTransformation & transf1, const FITKTransformation & transf2)
    //{
    //    return { transf1._Matrix * transf2._Matrix, transf1._Matrix * transf2._Vector + transf1._Vector };
    //}

    //std::array<double, 3> operator*(const FITKTransformation & transf, const std::array<double, 3>& point)
    //{
    //    auto p = Eigen::Vector3d(point[0], point[1], point[2]);
    //    auto newP = transf._Matrix * p + transf._Vector;
    //    return { newP[0] ,newP[1] ,newP[2] };
    //}



    FITKAssemblyInstance::FITKAssemblyInstance()
    {
        // 初始化变换工具。
        initTransformTool();
    }

    FITKAssemblyInstance::FITKAssemblyInstance(int modelId)
    {
        // 初始化变换工具。
        initTransformTool();

        setModel(modelId);
    }

    FITKAssemblyInstance::FITKAssemblyInstance(int modelId, double * xyz)
    {
        // 初始化变换工具。
        initTransformTool();

        setModel(modelId);

        if (m_transformTool)
        {
            // 获取初始变换信息。
            m_transformTool->transform(xyz[0], xyz[1], xyz[2]);
            m_transformTool->data(m_matrix, m_vector);
        }
        else
        {
            m_vector[0] = xyz[0];
            m_vector[1] = xyz[1];
            m_vector[2] = xyz[2];
        }
    }

    FITKAssemblyInstance::FITKAssemblyInstance(int modelId, double * pt1, double * pt2, double angle)
    {
        // 初始化变换工具。
        initTransformTool();

        setModel(modelId);

        if (m_transformTool)
        {
            // 获取初始变换信息。
            m_transformTool->rotate(pt1, pt2, angle);
            m_transformTool->data(m_matrix, m_vector);
        }
        else
        {
            for (int i = 0; i < 3; i++)
            {
                m_rotate1[i] = pt1[i];
                m_rotate2[i] = pt2[i];
            }

            m_angle = angle;
        }
    }

    FITKAssemblyInstance::FITKAssemblyInstance(FITKAssemblyInstance * instance, double * xyz)
    {
        //auto m4 = dynamic_cast<FITKAssemblyInstanceM4*>(instance);
        //auto tr = dynamic_cast<FITKAssemblyInstanceTR*>(instance);
        //auto other = dynamic_cast<FITKAssemblyInstance*>(instance);

        /*if (m4 != nullptr) {
            setModel(m4->getModelID());
            Eigen::Matrix3d matrix;
            matrix << m4->getTransformMatrix(0, 0), m4->getTransformMatrix(0, 1), m4->getTransformMatrix(0, 2),
                m4->getTransformMatrix(1, 0), m4->getTransformMatrix(1, 1), m4->getTransformMatrix(1, 2),
                m4->getTransformMatrix(2, 0), m4->getTransformMatrix(2, 1), m4->getTransformMatrix(2, 2);
            Eigen::Vector3d vector;
            vector << m4->getTransformMatrix(0, 3), m4->getTransformMatrix(1, 3), m4->getTransformMatrix(2, 3);
            _Transformation = FITKTransformation(matrix, vector);
        }
        else if (tr != nullptr)
        {
            setModel(tr->getModelID());
            double origin[3], secondPoint[3], direction[3];
            tr->getRotateAxis(origin, secondPoint);
            for (int i = 0; i < 3; ++i) {
                direction[i] = secondPoint[i] - origin[i];
            }
            _Transformation = FITKTransformation(origin, direction, tr->getAngle());
        }
        else if (other != nullptr)
        {
            _modelID = other->_modelID;
            _Transformation = other->_Transformation;
        }
        else {
            return;
        }
        transform(xyz[0], xyz[1], xyz[2]);*/

        // 初始化变换工具。
        initTransformTool();

        setModel(instance->getModelID());

        if (m_transformTool)
        {
            // 获取初始变换信息。
            m_transformTool->setData(instance->m_matrix, instance->m_vector);
            m_transformTool->transform(xyz[0], xyz[1], xyz[2]);
            m_transformTool->data(m_matrix, m_vector);
        }
        else
        {
            m_vector[0] = xyz[0];
            m_vector[1] = xyz[1];
            m_vector[2] = xyz[2];
        }
    }

    FITKAssemblyInstance::FITKAssemblyInstance(FITKAssemblyInstance * instance, double * pt1, double * pt2, double angle)
    {
        //auto m4 = dynamic_cast<FITKAssemblyInstanceM4*>(instance);
        //auto tr = dynamic_cast<FITKAssemblyInstanceTR*>(instance);
        //auto other = dynamic_cast<FITKAssemblyInstance*>(instance);

        //if (m4 != nullptr) {
        //    setModel(m4->getModelID());
        //    Eigen::Matrix3d matrix;
        //    matrix << m4->getTransformMatrix(0, 0), m4->getTransformMatrix(0, 1), m4->getTransformMatrix(0, 2),
        //        m4->getTransformMatrix(1, 0), m4->getTransformMatrix(1, 1), m4->getTransformMatrix(1, 2),
        //        m4->getTransformMatrix(2, 0), m4->getTransformMatrix(2, 1), m4->getTransformMatrix(2, 2);
        //    Eigen::Vector3d vector;
        //    vector << m4->getTransformMatrix(0, 3), m4->getTransformMatrix(1, 3), m4->getTransformMatrix(2, 3);
        //    _Transformation = FITKTransformation(matrix, vector);
        //}
        //else if (tr != nullptr)
        //{
        //    setModel(tr->getModelID());
        //    double origin[3], secondPoint[3], direction[3];
        //    tr->getRotateAxis(origin, secondPoint);
        //    for (int i = 0; i < 3; ++i) {
        //        direction[i] = secondPoint[i] - origin[i];
        //    }
        //    _Transformation = FITKTransformation(origin, direction, tr->getAngle());
        //}
        //else if (other != nullptr)
        //{
        //    _modelID = other->_modelID;
        //    _Transformation = other->_Transformation;
        //}
        //else {
        //    return;
        //}
        //rotate(axisOrigin, axisDir, angle);

        // 初始化变换工具。
        initTransformTool();

        setModel(instance->getModelID());

        if (m_transformTool)
        {
            // 获取初始变换信息。
            m_transformTool->setData(instance->m_matrix, instance->m_vector);
            m_transformTool->rotate(pt1, pt2, angle);
            m_transformTool->data(m_matrix, m_vector);
        }
        else
        {
            for (int i = 0; i < 3; i++)
            {
                m_rotate1[i] = pt1[i];
                m_rotate2[i] = pt2[i];
            }

            m_angle = angle;
        }
    }

    FITKAssemblyInstance::~FITKAssemblyInstance()
    {
        if (m_transformTool)
        {
            delete m_transformTool;
            m_transformTool = nullptr;
        }
    }

    void FITKAssemblyInstance::getMatrix4X4(double oMat[4][4])
    {
        if (m_transformTool == nullptr) return;
        double m[3][3], v[3];
        m_transformTool->data(m, v);

        oMat[0][0] = m[0][0];
        oMat[0][1] = m[0][1];
        oMat[0][2] = m[0][2];
        oMat[0][3] = v[0];

        oMat[1][0] = m[1][0];
        oMat[1][1] = m[1][1];
        oMat[1][2] = m[1][2];
        oMat[1][3] = v[1];

        oMat[2][0] = m[2][0];
        oMat[2][1] = m[2][1];
        oMat[2][2] = m[2][2];
        oMat[2][3] = v[2];

        oMat[3][0] = 0.;
        oMat[3][1] = 0.;
        oMat[3][2] = 0.;
        oMat[3][3] = 1.;
    }

    bool FITKAssemblyInstance::hasTransformTool()
    {
        return m_transformTool != nullptr;
    }

    void FITKAssemblyInstance::initTransformTool()
    {
        if (m_transformTool)
        {
            return;
        }

        // 获取工具生成器。
        Interface::FITKAbsAlgorithmToolsCreator* creator = Interface::FITKInterfaceAlgorithmFactory::getInstance()->getAlgToolsCreator();
        if (!creator)
        {
            return;
        }

        // 变换工具可重复使用。
        m_transformTool = creator->createTransformTool();
    }

    void FITKAssemblyInstance::transform(double x, double y, double z)
    {
        //Eigen::Vector3d vec;
        //vec << x, y, z;
        //_Transformation = FITKTransformation(vec) * _Transformation;

        //if (!m_transformTool)
        //{
        //    return;
        //}

        //// 初始旋转信息转换3x3矩阵。
        //double axisOrg[3]{ m_rotate2[0] - m_rotate1[0], m_rotate2[1] - m_rotate1[1], m_rotate2[2] - m_rotate1[2] };
        //double max33Org[3][3];
        //m_transformTool->rotationToMatrix3x3(m_rotate1, axisOrg, m_angle, max33Org);

        //double max33New[3][3];
        //double trans[3]{ x, y, z };
        //m_transformTool->multiply(max33Org, m_trans, trans, max33New, m_trans);

        //// 转换矩阵。
        //m_transformTool->matrix33ToRotation(max33New, m_rotate1, m_rotate2, m_angle);

        if (!m_transformTool)
        {
            return;
        }

        m_transformTool->setData(m_matrix, m_vector);
        m_transformTool->transform(x, y, z);
        m_transformTool->data(m_matrix, m_vector);
    }

    void FITKAssemblyInstance::rotate(double* pt1, double* pt2, double angle)
    {
        if (!m_transformTool)
        {
            return;
        }

        m_transformTool->setData(m_matrix, m_vector);
        m_transformTool->rotate(pt1, pt2, angle);
        m_transformTool->data(m_matrix, m_vector);

        /*ToolTransform iTrans1;
        ToolTransform iTrans2;
        ToolTransform oTrans;

        for (int i = 0; i < 3; i++)
        {
            iTrans1.Pt[i] = m_rotate1[i];
            iTrans1.Dir[i] = m_rotate2[i] - m_rotate1[i];
            iTrans1.Translate[i] = m_trans[i];

            iTrans2.Pt[i] = pt1[i];
            iTrans2.Dir[i] = pt2[i] - pt1[i];
        }

        iTrans1.Angle = m_angle;
        iTrans2.Angle = angle;

        m_transformTool->multiply(iTrans1, iTrans2, oTrans);

        for (int i = 0; i < 3; i++)
        {
            m_rotate1[i] = oTrans.Pt[i];
            m_rotate2[i] = m_rotate1[i] + oTrans.Dir[i];
            m_trans[i] = oTrans.Translate[i];
        }

        m_angle = oTrans.Angle;*/

        //// 初始旋转信息转换3x3矩阵。
        //double axisOrg[3]{ m_rotate2[0] - m_rotate1[0], m_rotate2[1] - m_rotate1[1], m_rotate2[2] - m_rotate1[2] };
        //double max33Org[3][3];
        //m_transformTool->rotationToMatrix3x3(m_rotate1, axisOrg, m_angle, max33Org);

        //double axis_2[3]{ pt2[0] - pt1[0], pt2[1] - pt1[1], pt2[2] - pt1[2] };
        //double max33_2[3][3];
        //m_transformTool->rotationToMatrix3x3(pt1, axis_2, angle, max33_2);

        //double max33New[3][3];
        //m_transformTool->multiply(max33Org, m_trans, max33_2, max33New, m_trans);

        //// 转换矩阵。
        //m_transformTool->matrix33ToRotation(max33New, m_rotate1, m_rotate2, m_angle);

        //_Transformation = FITKTransformation(axisOrigin, axisDir, angle) * _Transformation;
    }

    void FITKAssemblyInstance::getTransfom(double * transform)
    {
        // 获取工具生成器。
        if (m_transformTool)
        {
            Interface::FITKAbsAlgorithmToolsCreator* creator = Interface::FITKInterfaceAlgorithmFactory::getInstance()->getAlgToolsCreator();
            if (!creator)
            {
                return;
            }

            Interface::FITKAbstractTransformTool* tool = creator->createTransformTool();

            double p1[3], p2[3], angle, vec[3]{ 0., 0., 0. };
            bool flag = m_transformTool->isRotation(p1, p2, angle);
            if (flag)
            {
                tool->rotate(p1, p2, angle);
                double mat33[3][3]{ 1.,0.,0.,0.,1.,0.,0.,0.,1. };
                tool->data(mat33, vec);
            }

            for (int i = 0; i < 3; i++)
            {
                transform[i] = m_vector[i] - vec[i];
            }

            delete tool;
        }
        else
        {
            transform[0] = m_vector[0];
            transform[1] = m_vector[1];
            transform[2] = m_vector[2];
        }

        // m_transform->GetPosition(transform);

        //const auto vec = _Transformation.vector();
        //transform[0] = vec[0];
        //transform[1] = vec[1];
        //transform[2] = vec[2];
    }

    bool FITKAssemblyInstance::getRotateAxis(double * p1, double * p2, double * angle)
    {
        if (m_transformTool)
        {
            return m_transformTool->isRotation(p1, p2, *angle);
        }
        else
        {
            for (int i = 0; i < 3; i++)
            {
                p1[i] = m_rotate1[i];
                p2[i] = m_rotate2[i];
            }

            *angle = m_angle;

            return true;
        }

        return false;

        //m_transform->GetPosition(p1);

        //double wxyz[4]{ 0., 0., 0., 0. };
        //m_transform->GetOrientationWXYZ(wxyz);

        //*angle = wxyz[0];
        //
        //for (int i = 0; i < 3; i++)
        //{
        //    p2[i] = p1[i] + wxyz[i + 1] * 10;
        //}

        //return true;

        //double dir[3] = {};
        //if (_Transformation.isRotation(p1, dir, *angle)) {
        //    p2[0] = p1[0] + dir[0];
        //    p2[1] = p1[1] + dir[1];
        //    p2[2] = p1[2] + dir[2];
        //    return true;
        //}
        //return false;
    }

    void FITKAssemblyInstance::setTransform(double* trans, double* rotatePt1, double* rotatePt2, double angle)
    {
        // 执行变换并保存信息。
        if (m_transformTool)
        {
            m_transformTool->transform(trans[0], trans[1], trans[2]);
            m_transformTool->rotate(rotatePt1, rotatePt2, angle);
            m_transformTool->data(m_matrix, m_vector);
        }
        else
        {
            for (int i = 0; i < 3; i++)
            {
                m_vector[i] = trans[i];
                m_rotate1[i] = rotatePt1[i];
                m_rotate2[i] = rotatePt2[i];
            }

            m_angle = angle;
        }
    }

    bool FITKAssemblyInstance::getPointCoor(int pointID, double * coor, int modelIndex)
    {
        FITKAbstractModel* model = this->getModel();
        if (model == nullptr) return false;
        //部件坐标
        double pt[3] = { 0,0,0 };
        bool ok = model->getPointCoor(pointID, pt, modelIndex);
        if (!ok) return false;

        if (m_transformTool)
        {
            m_transformTool->transformPoint(pt, coor);
        }
        else
        {
            //空间变换
            //计算轴线
            double axis[3] = { 0,0,0 };
            for (int i = 0; i < 3; ++i) axis[i] = m_rotate2[i] - m_rotate1[i];

            //旋转
            Core::FITKPoint FKp(pt);
            Core::FITKVec3D FKAxis(axis);
            Core::FITKPoint res = Core::RotateAroundAxis(FKp, FKAxis, m_angle * FITK_PI / 180.0);
            res.getCoor(pt);

            //平移
            for (int i = 0; i < 3; ++i)
            coor[i] = pt[i] + m_vector[i];
        }

        return true;
    }

    void FITKAssemblyInstance::transformPoint(double* iPt, double* oPt)
    {
        if (!m_transformTool)
        {
            return;
        }

        m_transformTool->transformPoint(iPt, oPt);
    }

    void FITKAssemblyInstance::transformDirection(double* iDir, double* oDir)
    {
        if (!m_transformTool)
        {
            return;
        }

        m_transformTool->transformDirection(iDir, oDir);
    }

    //double FITKAssemblyInstance::getTransformMatrix(const int row, const int col)
    //{
    //    return m_transform->GetMatrix()->GetElement(row, col);
    //    //if (row < 3 && col < 3) { return _Transformation.matrix()(row, col); }
    //    //else if (col == 3 && row < 3) { return _Transformation.vector()(row); }
    //    //else if (row == 3 && col == 3) return 1.;
    //    //else return 0.;
    //}
}
